/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation

#define MAP_FILE_NAME "../../res/autmap-small.txt"
#define OCCU_RESOLUTION 0.05
#define J2B2_LOG_FILE "../../../data/j2b2_measurement.txt"


#define		MAXNBOFDATAPNTSFORMAPREGCOMPUT	5000
#define		MAXNBOFDATAPNTCELLS				10000000

int Nx,Ny;
float xx,yy,aa;
#define PI 3.1415926536
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}
void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
			if(v<0.6) v = 0.4;
			setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}
void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
	for(int ii=i-1;ii<i+1;ii++){
		for(int jj=j-1;jj<j+1;jj++){
			setRGBpix(rgb,ii,jj,Nx,r,g,b);
		}
	}
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
			 unsigned char r,unsigned char g,unsigned char b){
				 float dy = sin(angle);
				 float dx = cos(angle);
				 float rr=0;
				 int i=0;
				 uint x,y;
				 while(rr<length){
					 x = pix + i*dx;
					 y = piy - i*dy;
					 RGB[3*Nx*y+3*x] = r;
					 RGB[3*Nx*y+3*x+1] = g;
					 RGB[3*Nx*y+3*x+2] = b;
					 i++;
					 rr+=resolution;
				 }
}



typedef struct TwoDPointStruct {
/* Surface plane parameters */
	double	x;
	double	y;
} TwoDPoint;

typedef struct RegistrationPnt {
	TwoDPoint		*dataPnts;
	int			nbOfRegDataPnts;
	double		filteredDataPntDistanceFromMapCenter;
} RegistrationPntData;


typedef struct CellAlignmentSpanStruct {
	double				dx;
	double				dy;
	double				dphii;
} CellAlignmentSpan;

typedef struct CellAlignmentCentreStruct {
	double				x;
	double				y;
	double				phii;
} CellAlignment;

typedef struct CellAlignmentStruct {
	CellAlignmentSpan	cellAlignHalfSpan;
	CellAlignment		cellAlignCentre;
	double				upperBoundForCellScore;
	double				cellCenterScore;
} CellAlignmentData;

double xHalfSpanForStart = 50;
double yHalfSpanForStart = 50;
double phiiHalfSpanForStart = (180.0/180.0)*PI;
double initialx = 0.;
double initialy = 0.;
double initialphii = 0.;
double min_span = OCCU_RESOLUTION;
double ispan=1./(OCCU_RESOLUTION);
int		minReguiredRegDataPntNb = 5;


//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/**
* returns distance to nearest obstacle from position (x,y) (in the map coordinate system)
* @param *dt distance transform
* @param *g  pointer to occupancy grid
* @param x   x-coordinate
* @param y   y-coordinate
* @return distance to nearest obstacle
*/
float getDist(float *dt,TOccupancyGrid *g,float x, float y){
	int Nx,Ny;
	Grid::TPixel pix= g->toPixel(Grid::TCoord2D(x,y));
	g->getDim(Nx,Ny);
	
	if(pix.x >=0 && pix.x <Nx &&pix.y>=0&&pix.y<Ny){
		return dt[Nx*pix.y + pix.x];
	}
	return -1;
}

int	computeShortestDistanceBtwLocalMapElementWrtElementsInGlobalMap(
											TwoDPoint				pntInTheLocalMap,
											double					cosPhii,
											double					sinPhii,
											float*	globalMap,
											TOccupancyGrid *O,
											double					deltaC,
											TwoDPoint			currentAlignment,
											double*					distance,
											double*					best,
											double 					th)
{
	TwoDPoint		transfPnt;
	double			shortestDistance;
	double ssdmin,dist,di;
	int x,y;

/* Computed the coordinates of the transformed local map point */
	transfPnt.x = currentAlignment.x + pntInTheLocalMap.x*cosPhii - pntInTheLocalMap.y*sinPhii;
	transfPnt.y = currentAlignment.y + pntInTheLocalMap.x*sinPhii + pntInTheLocalMap.y*cosPhii;
	
	

/* Find the shortest distance of the transformed point w.r.t the points in the global map */
				
  //     dist=getDist(globalMap, O, transfPnt.x,transfPnt.y);
	
	
	x=(int)(transfPnt.x);
	y=(int)(-transfPnt.y);

	if(x >=0 && x <Nx &&y>=0&&y<Ny){
		dist=globalMap[Nx*y + x];
	}
	else
//	if(dist==-1)
	{
	*distance=th;
	*best=th;
	return 0;
	}								
        shortestDistance=dist*dist;
	
	di=(dist-deltaC);
	if(di<0)
		di=0;
	ssdmin=di*di;
	
	if(shortestDistance>th)
		shortestDistance=th;
		
	*distance = shortestDistance;
	
	if(ssdmin>th)
		ssdmin=th;
	
	*best=ssdmin;

	return(0);
}



int	computeDistanceToBeAddedToComputeUpperBoundForASingleElementScore(
											TwoDPoint			pntInTheLocalMap,
											double				deltaXYZC,
											double				sinPhiiPer2,
											double				cosPhii,
											double				sinPhii,
											CellAlignmentSpan	cellAlignHalfSpan,
											double*				deltaC )
{	
	double	distance;
	double	deltaPhiiC;



	distance = sqrt(pntInTheLocalMap.x*pntInTheLocalMap.x + 
					pntInTheLocalMap.y*pntInTheLocalMap.y);



	deltaPhiiC = fabs(2.*distance*sinPhiiPer2);






/* 3.1.01 */ 	*deltaC = (deltaXYZC + deltaPhiiC); 



	return(0);
}


int	calc_best_worst( 
							RegistrationPntData*	localMap,
							double*	r,
							float*	globalMap,
							TOccupancyGrid *O,
							CellAlignmentData*		cellAlignmentData,
							double*					upperBoundAvgLikelihood,
							double*					Likelihood,
							double					th )
{
	double	cosPhii;
	double	sinPhii;
	double	deltaSinPhiiPer2;
	double	deltaXYZC;
	double	deltaC;
	double	distance;
	double	accumulatedLikelihoodb;
	double	accumulatedLikelihoodw;
	double span,span2;
	static struct timespec start, stop;
	int		i;
	TwoDPoint center;

	double best,runtime;

//	clock_gettime(CLOCK_REALTIME,&start);
	cosPhii = cos((cellAlignmentData->cellAlignCentre).phii)*ispan;
	sinPhii = sin((cellAlignmentData->cellAlignCentre).phii)*ispan;
	deltaSinPhiiPer2 = 2.*fabs(sin((cellAlignmentData->cellAlignHalfSpan).dphii/2.0));
	deltaXYZC = sqrt(	(cellAlignmentData->cellAlignHalfSpan).dx*(cellAlignmentData->cellAlignHalfSpan).dx +
						(cellAlignmentData->cellAlignHalfSpan).dy*(cellAlignmentData->cellAlignHalfSpan).dy);
	

	span=th+min_span;
	span2=span*span;
/* Reset the upper bound likelihood values */
	*upperBoundAvgLikelihood = 0.0;
	center.x=cellAlignmentData->cellAlignCentre.x*ispan+Nx/2.;
	center.y=cellAlignmentData->cellAlignCentre.y*ispan-Ny/2.;
	if((localMap->nbOfRegDataPnts > minReguiredRegDataPntNb))
	{
	/* 8.6.01, Compute upper bound by means of cone map data */
		accumulatedLikelihoodb = 0.0;
		accumulatedLikelihoodw = 0.0;
		for(i=0; i<localMap->nbOfRegDataPnts; i++)
		{

			deltaC = deltaXYZC+r[i]*deltaSinPhiiPer2;
			
			computeShortestDistanceBtwLocalMapElementWrtElementsInGlobalMap(localMap->dataPnts[i], cosPhii, sinPhii,globalMap,O,deltaC,
										center, &distance,&best,span2);
			
		
			accumulatedLikelihoodw += distance;
			accumulatedLikelihoodb += best;
		}
		
		*upperBoundAvgLikelihood = accumulatedLikelihoodb/(localMap->nbOfRegDataPnts);
		(*Likelihood) = accumulatedLikelihoodw/(localMap->nbOfRegDataPnts);
	}

  //	clock_gettime(CLOCK_REALTIME,&stop);	
//	runtime = (stop.tv_sec - start.tv_sec)*1000 + (double)(stop.tv_nsec - start.tv_nsec) / 1000000.0;
	
	cellAlignmentData->upperBoundForCellScore = *upperBoundAvgLikelihood;
	cellAlignmentData->cellCenterScore = (*Likelihood);
	
	return(0);
}


CellAlignmentData*		divideEachCellInTheCurrentLevelIntoTwoSubCells( 
											CellAlignmentData*	cellAlignmentData,
											unsigned int index,
											unsigned int		*indexLess,
											unsigned int		*indexMore,
											unsigned int		*nbOfCells,
											double	filteredDataPntDistanceWithinTheLocalMap,
											double	*maxSpan)
{

	double	newXLess=0;
	double	newXMore=0;
	double	newYLess=0;
	double	newYMore=0;
	double	newPhiiLess=0;
	double	newPhiiMore=0;

	double	newXHalfSpan=0;
	double	newYHalfSpan=0;
	double	newPhiiHalfSpan=0;

	double	phiiHalfSpanEqualInMeters=0,span=0;

	CellAlignment		alignmentCentre;
	CellAlignmentSpan	alignmentSpan;


/* Compute a value in [meters] for the heading angle half span */
	phiiHalfSpanEqualInMeters = 
		fabs(2*filteredDataPntDistanceWithinTheLocalMap*sin(cellAlignmentData[index].cellAlignHalfSpan.dphii/2.0));

/* Check which one of the original translational half spans is the largest */
	if((cellAlignmentData[index].cellAlignHalfSpan.dx >= cellAlignmentData[index].cellAlignHalfSpan.dy) &&
		 (cellAlignmentData[index].cellAlignHalfSpan.dx >= phiiHalfSpanEqualInMeters))
	{
		newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx/2.0;
		newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy;
		newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii; 

		newXLess = cellAlignmentData[index].cellAlignCentre.x - newXHalfSpan;
		newXMore = cellAlignmentData[index].cellAlignCentre.x + newXHalfSpan;
		newYLess = cellAlignmentData[index].cellAlignCentre.y;
		newYMore = cellAlignmentData[index].cellAlignCentre.y;
		newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii;
		newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii;
		span=cellAlignmentData[index].cellAlignHalfSpan.dx;
	}
	else if((cellAlignmentData[index].cellAlignHalfSpan.dy >= cellAlignmentData[index].cellAlignHalfSpan.dx) &&
		 (cellAlignmentData[index].cellAlignHalfSpan.dy >= phiiHalfSpanEqualInMeters))
	{
		newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx;
		newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy/2.0;
		newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii; 

		newXLess = cellAlignmentData[index].cellAlignCentre.x;
		newXMore = cellAlignmentData[index].cellAlignCentre.x;
		newYLess = cellAlignmentData[index].cellAlignCentre.y - newYHalfSpan;
		newYMore = cellAlignmentData[index].cellAlignCentre.y + newYHalfSpan;
		newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii;
		newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii;
		span=cellAlignmentData[index].cellAlignHalfSpan.dy;
	}
	else if((phiiHalfSpanEqualInMeters >= cellAlignmentData[index].cellAlignHalfSpan.dx) &&
		(phiiHalfSpanEqualInMeters >= cellAlignmentData[index].cellAlignHalfSpan.dy) )
	{
		newXHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dx;
		newYHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dy;
		newPhiiHalfSpan = cellAlignmentData[index].cellAlignHalfSpan.dphii/2.0; 

		newXLess = cellAlignmentData[index].cellAlignCentre.x;
		newXMore = cellAlignmentData[index].cellAlignCentre.x;
		newYLess = cellAlignmentData[index].cellAlignCentre.y;
		newYMore = cellAlignmentData[index].cellAlignCentre.y;
		newPhiiLess = cellAlignmentData[index].cellAlignCentre.phii - newPhiiHalfSpan;
		newPhiiMore = cellAlignmentData[index].cellAlignCentre.phii + newPhiiHalfSpan;
		span=phiiHalfSpanEqualInMeters;
	}

	(*maxSpan)=span;
	
	alignmentSpan.dx = newXHalfSpan;
	alignmentSpan.dy = newYHalfSpan;
	alignmentSpan.dphii = newPhiiHalfSpan; //21.12.00
/* Now, create and add new cells into the global array */
/* First */
	alignmentCentre.x = newXLess;
	alignmentCentre.y = newYLess;
	alignmentCentre.phii = newPhiiLess; //21.12.00
	(*indexLess)=(*nbOfCells);
	cellAlignmentData[(*indexLess)].cellAlignCentre=alignmentCentre;
	cellAlignmentData[(*indexLess)].cellAlignHalfSpan=alignmentSpan;
	(*nbOfCells)++;


	/* Last */
	alignmentCentre.x = newXMore;
	alignmentCentre.y = newYMore;
	alignmentCentre.phii = newPhiiMore; //21.12.00
	(*indexMore)=(*nbOfCells);
	cellAlignmentData[(*indexMore)].cellAlignCentre=alignmentCentre;
	cellAlignmentData[(*indexMore)].cellAlignHalfSpan=alignmentSpan;
	(*nbOfCells)++;

	return(0);
}

/*min-heap remove and return minimum value node */
void up_heap(double *heap,unsigned int *indi, double val, unsigned int ind,unsigned int *N)
{
    unsigned int parent,child,tmpi;
    double tmp;

    heap[*N]=val;
    indi[*N]=ind;

    child=*N;
    (*N)++;
    if(*N==1)
        return;
    for(;;)
    {
        parent=(child-1)>>1;
        if(heap[parent]>heap[child])
        {
            tmp=heap[parent];
            heap[parent]=heap[child];
            heap[child]=tmp;
            tmpi=indi[parent];
            indi[parent]=indi[child];
            indi[child]=tmpi;

            if(parent==0)
                break;
        }
        else
            break;
        child=parent;
    }
}
void down_heap(double *heap,unsigned int *indi, double *val,unsigned  int *ind, unsigned int *N)
{
    unsigned int parent,child1,child2,tmpi;
    double tmp;

    (*N)--;
    *val=heap[0];
    *ind=indi[0];
    heap[0]=heap[*N];
    indi[0]=indi[*N];

    parent=0;
    child1=(parent<<1)+1;
    child2=(parent<<1)+2;
    for(;;)
    {
        if(child1>(*N))
            break;
        if(child2>(*N))
            child2=child1;

        if(heap[child1]<heap[child2])
        {
            if(heap[child1]<heap[parent])
            {
                tmp=heap[parent];
                heap[parent]=heap[child1];
                heap[child1]=tmp;
                tmpi=indi[parent];
                indi[parent]=indi[child1];
                indi[child1]=tmpi;

                parent=child1;
                child1=(parent<<1)+1;
                child2=(parent<<1)+2;
            }
            else
                break;
        }
        else
        { 
            if(heap[child2]<heap[parent])
            {
                tmp=heap[parent];
                heap[parent]=heap[child2];
                heap[child2]=tmp;
                tmpi=indi[parent];
                indi[parent]=indi[child2];
                indi[child2]=tmpi;
                parent=child2;
                child1=(parent<<1)+1;
                child2=(parent<<1)+2;
            }
            else
                break;
        }
    }
}

void  BetBloc(	double *nn,
		unsigned int *pb_ind,
		double *pb_cost,
		CellAlignmentData* 	cellAlignmentData,
		TOccupancyGrid *O,
		float*	globalMap,
		RegistrationPntData*	localMap,
		double *r,
		double filteredDataPntDistanceWithinTheLocalMap,
		double th
		)
{
    unsigned int i,j,ind1,ind2;
    unsigned int index,a,cnt=0,numnode=1;
    double best[2],worst[2],minworst,maxworst,val,max_span;

    j=0;
 
    calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[0],&best[0], &worst[0],th );

    minworst=worst[0];
    maxworst=best[0];
    printf("minworts:%f\n",minworst);
    printf("maxworts:%f\n",maxworst);
    a=0;
    up_heap(pb_cost,pb_ind,best[0],0, &j);

    down_heap(pb_cost,pb_ind,&val,&index,&j);
    while(val<=minworst)
    {
        /* 1. */

        divideEachCellInTheCurrentLevelIntoTwoSubCells( cellAlignmentData,index, &ind1,&ind2,&numnode,filteredDataPntDistanceWithinTheLocalMap,&max_span );
	
 	calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[ind1],&best[0], &worst[0],th );
        calc_best_worst(localMap,r,globalMap,O,&cellAlignmentData[ind2],&best[1], &worst[1],th );
     
    
	if(best[0]<val)
		best[0]=val;
	if(best[1]<val)
		best[1]=val;
	
	if(max_span>min_span)
		 if(best[0]<minworst)
            		up_heap(pb_cost,pb_ind,best[0],ind1, &j);
	
	if(worst[0]<minworst) /* 2. */
        {    
	    minworst=worst[0];
            a=ind1; 
	}	    
	if(max_span>min_span)    
        	if(best[1]<minworst)
	    		up_heap(pb_cost,pb_ind,best[1],ind2, &j);
	    
	if(worst[1]<minworst) /* 2. */
        {    
	    minworst=worst[1];
            a=ind2;
	}
	if(minworst<val)
		printf("error val: %f minworst: %f index :%d ind1: %d ind2: %d \n",val,minworst,index,ind1,ind2);
	cnt++;
	if((cnt%100000)==0)
	printf("nodenum:%d maxspan:%f b:%f val:%f w:%f mw:%f\n",numnode,max_span,best[0],val,worst[0],minworst);
	
	/* 3. */
	if(j==0)
		break;
	if(numnode>MAXNBOFDATAPNTCELLS-1)
		break;
        down_heap(pb_cost,pb_ind,&val,&index,&j);
	
    }
    printf("a:%d, ind1:%d,ind2:%d\n",(int)a,(int)ind1,(int)ind2);
    printf("index:%d\n",int(a));
    printf("span:%f\n",max_span);
    printf("minworts:%f\n",minworst); ///Score (pienempi parempi)
    printf("maxworts:%f\n",maxworst);
    printf("x:%f y:%f phii:%f\n",cellAlignmentData[a].cellAlignCentre.x,cellAlignmentData[a].cellAlignCentre.y,cellAlignmentData[a].cellAlignCentre.phii);
    printf("best0:%f\n",best[0]);
    printf("best1:%f\n",best[1]);
    printf("numnod:%d\n",numnode);
		
		xx  = cellAlignmentData[a].cellAlignCentre.x;
		yy  = cellAlignmentData[a].cellAlignCentre.y;
		aa  = cellAlignmentData[a].cellAlignCentre.phii;
    nn[0]=(double)a;
    nn[1]=max_span;
    nn[2]=minworst;
    if(0)
    {
    minworst=cellAlignmentData[0].cellCenterScore;
    maxworst=cellAlignmentData[0].cellCenterScore;
    val=cellAlignmentData[0].cellCenterScore;
    for(i=0;i<numnode;i++)
    {
 	//if(minworst<cellAlignmentData[i].cellCenterScore minworst=cellAlignmentData[i].cellCenterScore;
	//if(maxworst>cellAlignmentData[i].cellCenterScore) maxworst=cellAlignmentData[i].cellCenterScore;
    //	val+=cellAlignmentData[i].cellCenterScore;
	  printf("node:%f %f %f %f %f %f\n",cellAlignmentData[i].cellAlignCentre.x,cellAlignmentData[i].cellAlignCentre.y,cellAlignmentData[i].cellAlignCentre.phii,
	  cellAlignmentData[i].upperBoundForCellScore,cellAlignmentData[i].cellAlignHalfSpan.dx,cellAlignmentData[i].cellCenterScore);
    }
    for(i=0;i<Nx;i++)
    	for(j=0;j<Ny;j++)
	     printf("map: %d %d %f\n",i,j,globalMap[i*Ny+j]);
    }
    printf("mintime:%f\n",minworst);
    printf("maxtime:%f\n",maxworst);
    printf("meantime:%f\n",val/numnode);
    
    
    return;
}

int	viewPointRegistration(CellAlignmentData* cellAlignmentData, double *nn,TOccupancyGrid *O,float *globalMap,RegistrationPntData *localMap,double *r, double th, double fdist)
{

	double 		*pb_cost;
	unsigned int	*pb_ind;

	if (( pb_ind=(unsigned int*)malloc(MAXNBOFDATAPNTCELLS*sizeof(int))) == NULL) printf("malloc error");
	if (( pb_cost=(double*)malloc(MAXNBOFDATAPNTCELLS*sizeof(double))) == NULL) printf("malloc error");
 
	BetBloc(nn,pb_ind,pb_cost,cellAlignmentData,O,globalMap,localMap,r,fdist,th);

	free(pb_ind);
	free(pb_cost);
	return(0);
}


/// For reading the data from file
int readScanLine181(FILE *f,Grid::TScan &meas, Grid::pose &p){

		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}


/******************************************************/
int main(int argc, char *argv[]){
  
    RegistrationPntData   pts;
    TwoDPoint pt[181];
    CellAlignmentData *cellAlignmentData;
    CellAlignment zeroAlignment;
    CellAlignmentSpan initialSearchSpaceSpan;
    double ind[5],th=0.6,fdist=4.,r[181];
    float *dist=NULL,maxdist;
    int    numPoints;
    
    TOccupancyGrid *O; ///< Occupancy Grid 	
    
		///Reading the data from log file to odom and scan
		FILE *data_log = fopen(J2B2_LOG_FILE,"rt");
		Grid::pose odom;
		Grid::TScan scan;
		scan.alloc(181);
		
    
    
    cellAlignmentData = (CellAlignmentData *)calloc( MAXNBOFDATAPNTCELLS, sizeof( CellAlignmentData ) );
	

    /**
    * Initialize the occupancy grid map
    */
    O = new TOccupancyGrid(1,1,OCCU_RESOLUTION,Grid::TCoord2D(0,0));
    if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
	fprintf(stderr,"**** Unable to load the map\n");
	exit(1);
    }
    O->getDim(Nx,Ny); ///<Number of pixels in the occupancy grid
    dist = (float *) malloc(Nx*Ny*sizeof(float)); /// memory for distance transform

    /* Check for proper number of arguments */
    maxdist=O->distanceTransform(dist, 0.9); ///< Compute distance transform	
    printf("maxdist:%f Nx:%d Ny:%d\n",maxdist,Nx,Ny);

    /* Create the first cell in the search space */
    initialSearchSpaceSpan.dx = xHalfSpanForStart;
    initialSearchSpaceSpan.dy = yHalfSpanForStart;
    initialSearchSpaceSpan.dphii = phiiHalfSpanForStart;
    zeroAlignment.x = initialx;
    zeroAlignment.y = initialy;
    zeroAlignment.phii = initialphii;

    cellAlignmentData[0].cellAlignCentre=zeroAlignment;
    cellAlignmentData[0].cellAlignHalfSpan=initialSearchSpaceSpan;
    
    ///Read scan.
   for(int i=0;i<550;i++) readScanLine181(data_log,scan, odom);
   // float dist1=getDist(dist, O, 0, 0 );
   // printf("dist: %f\n",dist1);
   // O->getScan(-10.5,0.5,-PI/2,181,PI/180.0,0.9,scan);

    numPoints = scan.N; 	
    int j=0;
    for(int i=0;i<scan.N;i++){
    	if(scan.r[i]>0){
				pt[j].x = scan.r[i]*cos(scan.a[i]);
				pt[j].y = scan.r[i]*sin(scan.a[i]);
				//printf("pts: %f %f\n",pt[j].x,pt[j].y);
				r[j]=sqrt(pt[j].x*pt[j].x+pt[j].y*pt[j].y);
				j++;
			}		
		}
    pts.dataPnts=pt; 
    pts.nbOfRegDataPnts= j;
    long int ss1,us1,ss2,us2,sec,us,ms;
    float fms;
    ownTime_GetTimeOfDay(&ss1,&us1);
    viewPointRegistration(cellAlignmentData,ind,O,dist,&pts,r,th,fdist);
    ownTime_GetTimeOfDay(&ss2,&us2);
		
		
		sec = ss2-ss1;
    us = us2-us1;
    ms = sec*1000+us/1000;
    fms=(float)sec*1000.0+(float)us/1000.0;
    fprintf(stderr,"Done time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		
		/**
		 * These are for displaying only
		 */
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny); ///< the image
		addImageWindow(Nx, Ny); ///< initialize imageGUI
		glutImageGUI_Start();   ///< Start the imagegui
		setRGBmap(rgb,O, 220,220, 220); ///< set the map to image
		displayImage(0, rgb,Ny,Nx); ///< and display
		
		for(int i=0;i<scan.N;i++){
			Grid::TPixel pix = O->toPixel(Grid::TCoord2D(scan.r[i]*cos(scan.a[i]+aa)+xx,scan.r[i]*sin(scan.a[i]+aa)+yy));
			setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0);
		}
		displayImage(0, rgb,Ny,Nx); ///< and display
		
		
		fprintf(stderr,"Le Map\n");
		usleep(5*1000*1000);
	
		
		
		
}
